#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import time
import rospy

from arm import Arm
from img_tools import ImgTools

from sensor_msgs.msg import Joy


class Teleop():
    def __init__(self):
        self.arm = Arm()
        # self.img_tools = ImgTools()
        self.joy_sub = rospy.Subscriber('/joy', Joy, self.joy_callback, queue_size=1)

        rospy.spin()

    def joy_callback(self, joy_data):
        rospy.logdebug('joy callback...')
        rospy.logdebug('joy data is: %s', joy_data)

        # 自定义机械臂触发键
        if joy_data.axes[5] == -1:
            if joy_data.axes[7] == -1:
                self.arm_detect()
            elif joy_data.axes[7] == 1:
                self.arm_pick()
            elif joy_data.axes[6] == 1 or joy_data.axes[6] == -1:
                self.arm_place()
        # 自定义触发键
        elif joy_data.axes[2] == -1:
            if joy_data.buttons[0] == 1:
                self.img_save()

    def arm_detect(self):
        rospy.loginfo('[teleop] joy arm detect...')

        self.arm.arm_detect()

    def arm_pick(self):
        rospy.loginfo('[teleop] joy arm pick...')

        self.arm.arm_pick()

    def arm_place(self):
        rospy.loginfo('[teleop] joy arm place...')

        self.arm.arm_place()

    def img_save(self):
        rospy.loginfo('[teleop] joy img save...')

        self.img_tools.img_save()


if __name__ == '__main__':
    rospy.init_node('npu_robot_teleop', anonymous=True, log_level=envs.log_level)
    teleop = Teleop()
